#include "SE2RigidBodyPlanningValidityCheckCount.h"
#include "FCLStateValidityCheckerCounter.h"

using namespace ompl::app;

SE2RigidBodyPlanningValidityCheckCount::SE2RigidBodyPlanningValidityCheckCount()
: ompl::app::SE2RigidBodyPlanning()
{
}

SE2RigidBodyPlanningValidityCheckCount::~SE2RigidBodyPlanningValidityCheckCount()
{
}

void SE2RigidBodyPlanningValidityCheckCount::setup(void)
{
	inferEnvironmentBounds();

    if (AppTypeSelector<GEOMETRIC>::SimpleSetup::getProblemDefinition()->getStartStateCount() == 0)
    {
        OMPL_INFORM("Adding default start state");
        AppTypeSelector<GEOMETRIC>::SimpleSetup::addStartState(getDefaultStartState());
    }

    inferProblemDefinitionBounds();

    const ompl::base::StateValidityCheckerPtr &svc = allocStateValidityCheckerCount(AppTypeSelector<GEOMETRIC>::SimpleSetup::si_,
                                                                            getGeometricStateExtractor(), isSelfCollisionEnabled());
    if (AppTypeSelector<GEOMETRIC>::SimpleSetup::si_->getStateValidityChecker() != svc)
        AppTypeSelector<GEOMETRIC>::SimpleSetup::si_->setStateValidityChecker(svc);

    AppTypeSelector<GEOMETRIC>::SimpleSetup::getStateSpace()->setup();

    if (!AppTypeSelector<GEOMETRIC>::SimpleSetup::getStateSpace()->hasDefaultProjection())
        AppTypeSelector<GEOMETRIC>::SimpleSetup::getStateSpace()->
            registerDefaultProjection(allocGeometricStateProjector(AppTypeSelector<GEOMETRIC>::SimpleSetup::getStateSpace(),
                                                                    mtype_, getGeometricComponentStateSpace(),
                                                                    getGeometricStateExtractor()));

    AppTypeSelector<GEOMETRIC>::SimpleSetup::setup();
}

const ompl::base::StateValidityCheckerPtr& SE2RigidBodyPlanningValidityCheckCount::allocStateValidityCheckerCount(
	const ompl::base::SpaceInformationPtr &si, const ompl::app::GeometricStateExtractor &se, bool selfCollision)
{
	if (validitySvc_)
        return validitySvc_;

    GeometrySpecification geom = getGeometrySpecification();

    switch (ctype_)
    {
#if OMPL_HAS_PQP
        case PQP:
            if (mtype_ == Motion_2D)
                validitySvc_.reset (new PQPStateValidityChecker<Motion_2D>(si, geom, se, selfCollision));
            else
                validitySvc_.reset (new PQPStateValidityChecker<Motion_3D>(si, geom, se, selfCollision));
            break;
#endif
        case FCL:
            if (mtype_ == Motion_2D)
                validitySvc_.reset (new FCLStateValidityCheckerCounter<Motion_2D>(si, geom, se, selfCollision));
            else
                validitySvc_.reset (new FCLStateValidityCheckerCounter<Motion_3D>(si, geom, se, selfCollision));
            break;

        default:
            OMPL_ERROR("Unexpected collision checker type (%d) encountered", ctype_);
    };

    return validitySvc_;
}